#!/usr/bin/env python
#coding=utf-8

import rospy, time
import sys, rospy, tf, actionlib, moveit_commander
from gazebo_msgs.srv import *
from geometry_msgs.msg import *
import fetch_prepare
import fetch_teleop_key
import pick_up_item, put_item

if __name__ == "__main__":
    rospy.init_node('reset_racecar')

    rospy.wait_for_service('/gazebo/set_model_state')
    get_state_service = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) # 获取模型位置服务
    set_state_service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) # 设置模型位置服务

    # 重置fetch
    print("-"*20 + "Reset the fetch" + "-"*20)
    fetch_prepare.main()
    objstate = SetModelStateRequest()
    objstate.model_state.model_name = "fetch"
    objstate.model_state.pose.position.x = 5.8
    objstate.model_state.pose.position.y = 4.593
    objstate.model_state.pose.position.z = 0
    objstate.model_state.pose.orientation.w = 0.707
    objstate.model_state.pose.orientation.x = 0
    objstate.model_state.pose.orientation.y = 0
    objstate.model_state.pose.orientation.z = 0.707
    objstate.model_state.twist.linear.x = 0
    objstate.model_state.twist.linear.y = 0
    objstate.model_state.twist.linear.z = 0
    objstate.model_state.twist.angular.x = 0
    objstate.model_state.twist.angular.y = 0
    objstate.model_state.twist.angular.z = 0
    objstate.model_state.reference_frame = "world"
    result = set_state_service(objstate)
    fetch_teleop_key.fetch_stay()        # 使fetch静止, 要不会顺时针转动
    time.sleep(1)
    # 重置物块
    print("-"*20 + "Reset the objects" + "-"*20)
    ModelNames = ["product_0_clone", "product_0_clone_0", "product_0_yellow", "product_0_clone_1", "product_0_clone_2"]
    posx = 6.18
    posy = 5.34
    posz = 1.024940
    for i in ModelNames:
        objstate = SetModelStateRequest()  # Create an object of type SetModelStateRequest
        posx -= 0.128
        objstate.model_state.model_name = i
        objstate.model_state.pose.position.x = posx
        objstate.model_state.pose.position.y = posy
        objstate.model_state.pose.position.z = posz
        objstate.model_state.pose.orientation.w = 1
        objstate.model_state.pose.orientation.x = 0
        objstate.model_state.pose.orientation.y = 0
        objstate.model_state.pose.orientation.z = 0
        objstate.model_state.twist.linear.x = 0.0
        objstate.model_state.twist.linear.y = 0.0
        objstate.model_state.twist.linear.z = 0.0
        objstate.model_state.twist.angular.x = 0.0
        objstate.model_state.twist.angular.y = 0.0
        objstate.model_state.twist.angular.z = 1.57
        objstate.model_state.reference_frame = "world"
        result = set_state_service(objstate)

    pick_up_item.main()
    put_item.main()

    while True:
        RaceCar = GetModelStateRequest()
        Yellow = GetModelStateRequest()
        RaceCar.model_name = "racecar"
        Yellow.model_name = "product_0_yellow"
        CarState = get_state_service(RaceCar)
        YellowState = get_state_service(Yellow)
        CarPosition = (CarState.pose.position.x, CarState.pose.position.y, CarState.pose.position.z)
        YellowPosition = (YellowState.pose.position.x, YellowState.pose.position.y, YellowState.pose.position.z)
        if CarPosition[0]<0.13 and CarPosition[0]>-0.07 and CarPosition[1]>5.68 and CarPosition[1]<5.8 \
            and YellowPosition[0]<0.3 and YellowPosition[0]>-0.24 and YellowPosition[1]>5.473 and YellowPosition[1]<6.02: # 终点
            print("Competition complete!")
            break